#include <plib/usart.h>
//Declaramos el PIC a utilizar
#include<p18f4550.h>
//Declaramos librerias necesarias para el uso de la LCD
#include<plib/xlcd.h>
#include<plib/delays.h>
# pragma config FOSC =HS , FCMEN =OFF , IESO =OFF , CPUDIV = OSC2_PLL3
# pragma config PWRT =ON , BOR =OFF , BORV =0, WDT =OFF , WDTPS =32768
# pragma config MCLRE =ON , LPT1OSC =OFF , PBADEN =OFF , CCP2MX = OFF
# pragma config STVREN =OFF , LVP =OFF , XINST =OFF , DEBUG = OFF
# pragma config CP0 =OFF , CP1 =OFF , CP2 =OFF , CPB =OFF , CPD = OFF
# pragma config WRT0 =OFF , WRT1 =OFF , WRT2 = OFF
# pragma config WRTB =OFF , WRTC =OFF , WRTD = OFF
# pragma config EBTR0 =OFF , EBTR1 =OFF , EBTR2 =OFF , EBTRB = OFF

//Retardos para operacion interna de la libreria
void DelayFor18TCY(){Delay10TCYx(2);}
void DelayPORXLCD(){Delay1KTCYx(15);}
void DelayXLCD(){Delay1KTCYx(5);}



unsigned char mide_distancia(void)//Funcion de medicion de distancia
{
     int centimetros;
    //Pulso de disparo
    PORTCbits.RC2=1;//Pin TRIG
    Delay1TCYx(15000);//Tiempo mayor a 10 micro segundos
    PORTCbits.RC2=0;
    Delay1TCYx(15000);//Tiempo mayor a 10 micro segundos

    while(PORTCbits.RC1==1)//Pin ECHO
    {
        centimetros++;
        Delay10TCYx(2);//Espera de 58 micro segundos tiempo en que tarda en viada el sonido 2cm.
    }
    Delay1KTCYx(5);
    return(centimetros+5);
}

void main()
{
    OpenUSART(USART_TX_INT_OFF & USART_RX_INT_OFF & USART_ASYNCH_MODE & USART_EIGHT_BIT & USART_CONT_RX & USART_BRGH_HIGH,25);
    int t,a,dato,datoa=0,dist,c,d,u, evitado=0;
        ADCON2=128;TXSTA=36; RCSTA=144;BAUDCON=0;SPBRG=25;// Configuracion comunicacion serial
    TRISD=0xf0;
    ADCON1=15;

    OpenXLCD(FOUR_BIT & LINES_5X7);//inicializa configuracion de la LCD a 4 bits
    //Motores
    TRISAbits.TRISA3=0;//Declarado como salida-IN1
    TRISAbits.TRISA4=0;//Declarado como salida-IN2
    TRISAbits.TRISA5=0;//Declarado como salida-IN3
    TRISCbits.TRISC0=0;//Declarado como salida-IN4

     TRISDbits.TRISD4=0;

    //Sensor de ultrasonido
    TRISCbits.TRISC2=0;//Declarado como salida-TRIG
    TRISCbits.TRISC1=1;//Declarado como entrada-ECHO

    //Sensores de linea negra
    TRISAbits.TRISA0=1;//Declarado como entrada-CNY70(DER)
    TRISAbits.TRISA1=1;//Declarado como entrada-CNY70(IZQ)
    TRISAbits.TRISA2=1;//Declarado como entrada-CNY70(CEN)

    evitado=0;

    while(1)
    {        WriteCmdXLCD(1);
             SetDDRamAddr(0x00);
             putrsXLCD("Distancia en cm");
             SetDDRamAddr(0x40);
             dist=mide_distancia();
                 c=dist/100;
                 d=(dist-(c*100))/10;
                 u=dist-(c*100)-(d*10);
             WriteDataXLCD(0x30+d);
             WriteDataXLCD(0x30+u);
             Delay1KTCYx(100);

             if(dist>18)//Si la distancia es menor a 18
             {
                //**** Sigue la linea negra ****//
                PORTDbits.RD4=0;//Led que indica si la distancia es menor a 18
                if(PORTAbits.RA0==0 && PORTAbits.RA1==0 )//Adelante
                {
                 PORTAbits.RA3=1;
                 PORTAbits.RA4=0;
                 PORTAbits.RA5=1;
                 PORTCbits.RC0=0;
                 

                }
                    else if(PORTAbits.RA0==0 && PORTAbits.RA1==1)//Giro a la derecha
                    {
                     PORTAbits.RA3=1;
                     PORTAbits.RA4=0;
                     PORTAbits.RA5=0;
                     PORTCbits.RC0=0;

                    }
                        else if(PORTAbits.RA0==1 && PORTAbits.RA1==0)//Giro a la izquierda
                        {
                        PORTAbits.RA3=0;
                        PORTAbits.RA4=0;
                        PORTAbits.RA5=1;
                        PORTCbits.RC0=0;

                        }
                            else if(PORTAbits.RA0==1 && PORTAbits.RA1==1)//Detenido
                            {
                            //Hacia adelante
                            PORTAbits.RA3=0;
                            PORTAbits.RA4=0;
                            PORTAbits.RA5=0;
                            PORTCbits.RC0=0;

                            }
                         //**** Fin de programa para seguir la linea negra ****//
             }
             else
             {
                 PORTDbits.RD4=1;//Led que indica si la distancia es menor a 18
                 while(PORTAbits.RA2==0)//Hace el ciclo hasta que el censor lateral detecte la linea negra
                 {          
                            //***Gira en su eje hacia la izquierda***//
                            PORTAbits.RA3=1;
                            PORTAbits.RA4=0;
                            PORTAbits.RA5=0;
                            PORTCbits.RC0=1;
                 }
                 while(PORTAbits.RA1==0)//Esquiva el obstacuilo girando hacia la derecha (La rueda derecha gira mas lento que la izquierda)
                 {
                            PORTAbits.RA3=1;
                            Delay1KTCYx(1000);
                            PORTAbits.RA3=0;
                            Delay10TCYx(12);
                            PORTAbits.RA4=0;
                            PORTAbits.RA5=1;
                            Delay1KTCYx(1000);
//                            PORTAbits.RA5=0;
//                            Delay1TCYx(12);
                            PORTCbits.RC0=0;
                  }

                   while(dist>5)//Ciclo infinito
                   {
                    //Seguidor de linea negra
                                                PORTDbits.RD4=0;
                                            if(PORTAbits.RA0==0 && PORTAbits.RA1==0 )//Adelante
                                            {
                                             PORTAbits.RA3=1;
                                             PORTAbits.RA4=0;
                                             PORTAbits.RA5=1;
                                             PORTCbits.RC0=0;


                                            }
                                                else if(PORTAbits.RA0==0 && PORTAbits.RA1==1)//Giro a la derecha
                                                {
                                                 PORTAbits.RA3=1;
                                                 PORTAbits.RA4=0;
                                                 PORTAbits.RA5=0;
                                                 PORTCbits.RC0=0;

                                                }
                                                    else if(PORTAbits.RA0==1 && PORTAbits.RA1==0)//Giro a la izquierda
                                                    {
                                                    PORTAbits.RA3=0;
                                                    PORTAbits.RA4=0;
                                                    PORTAbits.RA5=1;
                                                    PORTCbits.RC0=0;

                                                    }
                                                        else if(PORTAbits.RA0==1 && PORTAbits.RA1==1)//Detenido
                                                        {
                                                        //Hacia adelante
                                                        PORTAbits.RA3=0;
                                                        PORTAbits.RA4=0;
                                                        PORTAbits.RA5=0;
                                                        PORTCbits.RC0=0;

                            }
                   }

             }
//             if(dist<25)
//             {
//                 PORTDbits.RD4=1;
//             }
//             else
//             {
//                 PORTDbits.RD4=0;
//             }
//              
         }
    }




